#include "mymath.h"

#include <iostream>


MyMath::MyMath(){}



Eigen::MatrixXd    MyMath::my_Rv_2_RM(   Eigen::MatrixXd rvvv   )  // rv(3,1);   // Exponential Map - so(3)=>SO(3)     //   IGNORE T !!!!!!!!!!!!!!!!!! (zero pitch screw)
{

        Eigen::MatrixXd RRR(3,3);

        double theta = rvvv.norm();

        Eigen::MatrixXd I3 = Eigen::MatrixXd::Identity(3,3);

        Eigen::MatrixXd W(     3,1 );
        Eigen::MatrixXd W_hat( 3,3 );  // Wmega Hat


        double              thresh_shouldBeZero = 0.00009;
        if (theta < (double)thresh_shouldBeZero)
        {
                RRR = I3;
        }
        else
        {
                W = rvvv / theta;

                W_hat <<  0,     -W(2),   W(1),  // Wmega Hat
                          W(2),   0,     -W(0),
                         -W(1),   W(0),   0;

                RRR = I3 + W_hat*(double)qSin(theta) + (W_hat*W_hat)*(double)(1.0-qCos(theta)); // Rodrigues!!!
        }

        ///////////
        return RRR;
        ///////////

}


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


int MyMath::myRounding( double double_IN, int MAX)
{
        int intRoundVal = qRound( double_IN );

        if (intRoundVal <     0)    intRoundVal =     0;
        if (intRoundVal > MAX-1)    intRoundVal = MAX-1;

        return intRoundVal;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

